#include "serial.hpp"
#include <cstring>
#include <fcntl.h>
#include <ros/ros.h>
#include <termios.h>
#include <unistd.h>

uint8_t crc8(const void *vptr, int len) {
	const uint8_t *data = (const uint8_t *)vptr;
	unsigned crc = 0;
	int i, j;
	for (j = len; j; j--, data++) {
		crc ^= (*data << 8);
		for (i = 8; i; i--) {
			if (crc & 0x8000)
				crc ^= (0x1070 << 3);
			crc <<= 1;
		}
	}
	return (uint8_t)(crc >> 8);
}

#define MSG_HEADER ((char)0xaa)

DemoBoard::DemoBoard(const std::string &path) {
	fd = open("/dev/ttyUSB0", O_RDWR | O_NOCTTY | O_NONBLOCK);
	if (fd == -1)
		throw std::runtime_error("Failed to open serial port: " +
		                         std::string(std::strerror(errno)));

	termios tOption;
	tcgetattr(fd, &tOption);
	tOption.c_iflag = 0;
	tOption.c_oflag = 0;
	tOption.c_cflag = CREAD | CLOCAL | CS8;
	tOption.c_lflag = 0;
	cfsetispeed(&tOption, B115200);
	cfsetospeed(&tOption, B115200);
	tcsetattr(fd, TCSANOW, &tOption);
	tcflush(fd, TCIOFLUSH);
}

DemoBoard::~DemoBoard() {
	tcflush(fd, TCIOFLUSH);
	close(fd);
}

std::optional<incoming_msg> DemoBoard::poll_message() {
	ssize_t r = ::read(fd, read_buf, READ_BUF_SIZE);
	if (r == -1) {
		if (errno != EAGAIN) {
			ROS_WARN("Failed to read serial port: %s", std::strerror(errno));
		}
		return std::nullopt;
	}

	std::optional<incoming_msg> msg_received;
	for (size_t i = 0; i < r; i++) {
		if (received_byte(read_buf[i])) {
			msg_received = msg;
		}
	}
	return msg_received;
}

void DemoBoard::send_message(const outgoing_msg &msg_out) {
	uint8_t word = 0xaa;
	if (::write(fd, &word, 1) == -1) {
		ROS_WARN("Failed to write message header: %s", std::strerror(errno));
		return;
	}

	ssize_t r = ::write(fd, &msg_out, sizeof(outgoing_msg));
	if (r == -1) {
		ROS_WARN("Failed to write message payload: %s", std::strerror(errno));
		return;
	} else if (r < sizeof(outgoing_msg)) {
		ROS_WARN_STREAM("Message payload fully written, written: " << r);
		return;
	}

	word = crc8(&msg_out, sizeof(outgoing_msg));
	if (::write(fd, &word, 1) != 1) {
		ROS_WARN("Failed to write message checksum: %s", std::strerror(errno));
		return;
	}
    ROS_INFO_STREAM("msg written " << msg_out.led_on);
}

bool DemoBoard::received_byte(char b) {
	char *msg_ptr = (char *)&msg;
	if (msg_len == -1) {
		if (b == MSG_HEADER) {
			// received frame header
			msg_len = 0;
			return false;
		} else {
			// frame header not received, skipping
			return false;
		}
	} else if (msg_len < sizeof(incoming_msg)) {
		// receiving payload
		msg_ptr[msg_len++] = b;
		return false;
	} else {
		// received crc8 checksum
		uint8_t actual_crc8 = crc8(msg_ptr, sizeof(incoming_msg));
		if (actual_crc8 == b) {
			// checksum matches
			// prepare for next frame
			msg_len = -1;
			return true;
		} else {
			// checksum mismatch
			for (size_t i = 0; i < sizeof(incoming_msg); i++) {
				if (msg_ptr[i] == MSG_HEADER) {
					msg_len = sizeof(incoming_msg) - (i + 1);
					std::memmove(msg_ptr, msg_ptr + (i + 1), msg_len);
					msg_ptr[msg_len++] = b;
					return false;
				}
			}
			if (b == MSG_HEADER) {
				msg_len = 0;
				return false;
			} else {
				msg_len = -1;
				return false;
			}
		}
	}
}
